/*
 * step.c
 *
 *  Created on: Apr 20, 2021
 *      Author: soliber
 */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "step.h"
#include "tim.h"
#include <stdio.h>

/* Private variables ---------------------------------------------------------*/
//static int freq;
static int position;	//当前位置(pulse)
static int position_max;
static int step;		//剩余的步数
static uint8_t search_zero;	//找零，1进入找零模式，0则不进行找零

/* Exported functions --------------------------------------------------------*/

/**
 * @brief  Initializes the step motor and search zero position
 * @retval Status, 0 = OK, -1 = Error
 */
int Motor_Init(void) {
	Motor_Set_Speed(MOTOR_SPEED_MAX);
	//开始找零
	printf("Searching zero position...\r\n");
	//左移动
	search_zero = 1;
	Motor_Move_um(MOTOR_POS_UM_MAX);

	//右移动
	position_max = -1*Motor_Move_um(-1 * MOTOR_POS_UM_MAX);
	position = 0;
	printf("Zero position found\r\nMax position is %+.3f mm\r\n", (double)position_max/1000);
	return 0;
}


/**
 * @brief  Set current position to be origin
 */
void Motor_Set_Origin(void)
{
	position = 0;
}

/**
 * @brief	Get move speed
 * @retval	Real speed in um/s
 */
int Motor_Get_Speed(void)
{
	//speed(um/s) = MOTOR_SPEED_MAX(um/s)/(PRE+1)
	return MOTOR_SPEED_MAX/(htim1.Instance->PSC+1);
}

/**
 * @brief	Set move speed
 * @param	speed the move speed in um/s
 * @retval	Real speed
 */
int Motor_Set_Speed(int speed_um_s)
{
	//PRE = MOTOR_SPEED_MAX(um/s)/speed(um/s) -1
	htim1.Instance->PSC = MOTOR_SPEED_MAX/speed_um_s-1;
	return Motor_Get_Speed();
}

/**
 * @brief  Get absolute position in steps
 * @retval Real position to zero position in steps
 */
int Motor_Get_Pos_step(void) {
	return position;
}

/**
 * @brief  Get absolute position in steps
 * @retval Real position to zero position in um
 */
int Motor_Get_Pos_um(void) {
	return position * MOTOR_UM_PER_STEP;
}

/**
 * @brief  Move some steps
 * @param	dx_steps the steps want to move
 * @retval Real steps moved
 */
int Motor_Move_step(int dx_steps) {
	int res = position;

	//判断方向
	if (dx_steps > 0) {
		//没有到头才能移动
		if (HAL_GPIO_ReadPin(KL_GPIO_Port, KL_Pin) == GPIO_PIN_RESET) {
			//printf("Already at the MAX position!\r\n");
			return 0;
		}
		//设置方向
		HAL_GPIO_WritePin(DIR_GPIO_Port, DIR_Pin, GPIO_PIN_SET);
	} else if (dx_steps < 0) {
		//没有到头才能移动
		if (HAL_GPIO_ReadPin(KR_GPIO_Port, KR_Pin) == GPIO_PIN_RESET) {
			//printf("Already at the MIN position!\r\n");
			return 0;
		}
		//设置方向
		HAL_GPIO_WritePin(DIR_GPIO_Port, DIR_Pin, GPIO_PIN_RESET);
	} else {
		//不需要移动
		//printf("Don't need to move.\r\n");
		return 0;
	}

	//发送脉冲
	step = dx_steps;
	HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_1);
	//等待移动完成
	while (htim1.ChannelState[0] != HAL_TIM_CHANNEL_STATE_READY)
		;
	//返回实际移动距离
	res = position - res;
	//printf("Moved %d um\r\n",(int)(res*MOTOR_UM_PER_STEP));
	return res;
}

/**
 * @brief  Move some distance in um
 * @param	dx_um the distance want to move
 * @retval Real distance moved
 */
int Motor_Move_um(int dx_um) {
	int res;
	res = Motor_Move_step(dx_um / MOTOR_UM_PER_STEP);
	return res * MOTOR_UM_PER_STEP;
}

/**
 * @brief  Move to absolute position in step
 * @param	x_steps the absolute position in step
 * 		@arg	MOTOR_STEP_MIN <= x_steps <= MOTOR_STEP_MAX
 * @retval Real position in step
 */
int Motor_Pos_step(int x_steps) {
	return Motor_Move_step(x_steps - position);
}

/**
 * @brief  Move to absolute position in um
 * @param	dir x_um the absolute position in um
 * 		@arg	MOTOR_POS_UM_MIN <= x_um <= MOTOR_POS_UM_MAX
 * @retval Real position in um
 */
int Motor_Pos_um(int x_um) {
	Motor_Move_step(x_um / MOTOR_UM_PER_STEP - position);
	return position * MOTOR_UM_PER_STEP;
}


/* TIM Callback functions --------------------------------------------------------*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
	//右限位
	if (GPIO_Pin == KR_Pin) {
		//找零过程
		if (search_zero) {
				//设置位置为0
				search_zero = 0;
				//停止
				HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_1);
//				printf("Zero position found.\r\n");
		}
		//限位停止
		else{
			HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_1);
			printf("Right Limited.\r\n");
		}
	}
	//左限位
	else if (GPIO_Pin == KL_Pin) {
		//找零过程
		if(search_zero)
		{
			//停止
			HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_1);
//			printf("Max position found.\r\n");
		}
		//限位停止
		else{
			HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_1);
			printf("Left Limited.\r\n");
		}
	}
}

void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) {
	//tim1 overflow(10KHz)
	if (htim == &htim1) {
		//记录位置
		switch (HAL_GPIO_ReadPin(DIR_GPIO_Port, DIR_Pin)) {
		case GPIO_PIN_SET:
			step--;
			position++;
			break;
		case GPIO_PIN_RESET:
			step++;
			position--;
			break;
		}
		//走完了
		if (step == 0) {
			//步数走完即停止
			HAL_TIM_PWM_Stop_IT(htim, TIM_CHANNEL_1);
		}
	}
}
